#include "AddIMUHandlerSensorConfiguration.h"

using namespace MMM;

AddIMUHandlerSensorConfiguration::AddIMUHandlerSensorConfiguration() : AddAttachedSensorConfiguration<IMUSensor>("imu", {"AccelerationX", "AccelerationY", "AccelerationZ"})
{
}

void AddIMUHandlerSensorConfiguration::addSensorMeasurements(const std::vector<std::map<std::string, std::string> > &values) {
    for (std::map<std::string, std::string> map : values) {
        float timestep = XML::convertTo<float>(map[TIMESTEP_STR], "Timestep no valid float!");
        Eigen::Vector3f acceleration(XML::convertTo<float>(map["AccelerationX"], "Acceleration in x-direction no valid float!"),
                     XML::convertTo<float>(map["AccelerationY"], "Acceleration in y-direction no valid float!"),
                     XML::convertTo<float>(map["AccelerationZ"], "Acceleration in z-direction no valid float!"));
        IMUSensorMeasurementPtr measurement(new IMUSensorMeasurement(timestep, acceleration));
        sensor->addSensorMeasurement(measurement);
    }
}
